/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include<unistd.h>
#include <string>
#include <iostream>
#include <fstream> 
#include "CGridGServerModule.h"
#include "glutImageGUI.h"
#include "../imageclient/imageclientinterface/CImageClient.h"
#include "../stereoCR/StereoRoutines.h"

// #define SHOW_WAVES

std::string paramHubAdress="asrobo.hut.fi";
int paramHubPort=50002;
std::string paramOwnName="grid_server";
std::string paramImageServerName="stereo_fw_emb3_asrobo"; //fw_server
float minNumBuf=200;
float minStDev=0.0005;
int stDevWinMin=15;
int stDevWinMax=100;
int valueIndex=20;
std::string imageNum = "1234";
bool loadFromFile = false;
bool loadPGM = false;
int minDisp = 10;
int maxDisp = 100;
bool showDisparity = false;
bool showCameraImages = false;
int rightOffset = 30;

int paramDebugLevel = 10;

typedef float grid_t;

void  printUsage(){
	fprintf(stderr,"Grid server\n");
	fprintf(stderr,"Options:\n");
	fprintf(stderr,"-o string : set the own hub name\n");
	fprintf(stderr,"-d int : Set debug level\n");
	fprintf(stderr,"-p int : Set hub port\n");
	fprintf(stderr,"-h string : Hub name\n");
	fprintf(stderr,"-s string : Image server name\n");
	fprintf(stderr,"Samin virityksiä:\n");
	fprintf(stderr,"-i int : Debug image-pair number\n");
	fprintf(stderr,"-A float : minNumBuf (default=200)\n");
	fprintf(stderr,"-B float : minStDev (default=0.0005)\n");
	fprintf(stderr,"-C int : stDevWinMin (default=15)\n");
	fprintf(stderr,"-D int : stDevWinMax (default=100)\n");
	fprintf(stderr,"-E int : valueIndex (default=20)\n");
	fprintf(stderr,"-F int : minimum disparity (default=10)\n");
	fprintf(stderr,"-G int : maximum disparity (default=100)\n");
	fprintf(stderr,"-H int : show camera images (0/1, default=0)\n");
	fprintf(stderr,"-I int : show disparity image (0/1, default=0)\n");
	fprintf(stderr,"-J int : right image offset (default=30)\n");
	fprintf(stderr,"-? Usage\n");
	fprintf(stderr,"---------------------------------------\n");
}

void parse(int argc,char **argv){
	bool paramParseLoop = true;
	int option_char;
	/*** Set options ***/
	while (paramParseLoop) {
		option_char = getopt(argc, argv, "o:d:h:p:s:i:m:A:B:C:D:E:F:G:H:I:J:");
		switch (option_char) {
			case 'o':
				if (optarg != NULL)
					paramOwnName = optarg;
				fprintf(stderr,"Setting own name to %s\n",paramOwnName.c_str());
				break;
			case 'd':
				if (optarg != NULL)
					paramDebugLevel = atoi (optarg);
				fprintf(stderr,"Debug level=%d\n",paramDebugLevel);
				break;
			case 'h':
				if (optarg != NULL)
					paramHubAdress = optarg;
			  
				fprintf(stderr,"Hub adress set to %s\n",paramHubAdress.c_str());
				break;
			case 's':
				if (optarg != NULL)
					paramImageServerName = optarg;
			  
				fprintf(stderr,"Image server name set to %s\n",paramImageServerName.c_str());
				break;
			case 'p':
				if (optarg != NULL)
					paramHubPort = atoi (optarg);
				fprintf(stderr,"Hub port set to %d\n",paramHubPort);
				break;
			case 'i':
				if (optarg != NULL)
					imageNum = optarg;
				fprintf(stderr,"Image number set to %s\n",imageNum.c_str());
				loadFromFile = true;
				break;
			case 'A':
			  if (optarg != NULL)
			    minNumBuf = atof (optarg);
				fprintf(stderr,"minNumBuf set to %.5f\n",minNumBuf);
  			break;
			case 'B':
			  if (optarg != NULL)
			    minStDev = atof (optarg);
				fprintf(stderr,"minStDev set to %.10f\n",minStDev);
  			break;
			case 'C':
			  if (optarg != NULL)
			    stDevWinMin = atoi (optarg);
				fprintf(stderr,"stDevWinMin set to %d\n",stDevWinMin);
  			break;
			case 'D':
			  if (optarg != NULL)
			    stDevWinMax = atoi (optarg);
				fprintf(stderr,"stDevWinMax set to %d\n",stDevWinMax);
  			break;
			case 'E':
			  if (optarg != NULL)
			    valueIndex = atoi (optarg);
				fprintf(stderr,"valueIndex set to %d\n",valueIndex);
  			break;
			case 'F':
			  if (optarg != NULL)
			    minDisp = atoi (optarg);
				fprintf(stderr,"Minimum disparity set to %d\n",minDisp);
  			break;
			case 'G':
			  if (optarg != NULL)
			    maxDisp = atoi (optarg);
				fprintf(stderr,"Maximum disparity set to %d\n",maxDisp);
  			break;
			case 'H':
			  if (optarg != NULL)
			    showCameraImages = (atoi (optarg) != 0);
			  if (showCameraImages)
    				fprintf(stderr,"Showing camera images\n");
    		else
    				fprintf(stderr,"Not showing camera images\n");
  			break;
			case 'I':
			  if (optarg != NULL)
			    showDisparity = (atoi (optarg) != 0);
			  if (showDisparity)
    				fprintf(stderr,"Showing disparity image\n");
    		else
    				fprintf(stderr,"Not showing disparity image\n");
  			break;
			case 'J':
			  if (optarg != NULL)
			    rightOffset = atoi (optarg);
				fprintf(stderr,"Right offset set to %d\n", rightOffset);
  			break;
			case '?':
				printUsage();
				exit(1);
				break;
			case -1:
				paramParseLoop = false;
				break;
		}
	}
}


char * loadRawImage(const char * filename)
{
    std::ifstream::pos_type size;
    char memblock[100]={0};
    char chrBuf;
    int i, j, counter, k, pos;
    int width=0, height=0;
    char terminalSequence[3]={0,0,0};
    int terminalSequenceLength=0;
    bool isComment = false;
    char * YUV = NULL;
    bool isGrayscale = false;
 
    std::ifstream file(filename, std::ios::in|std::ios::binary|std::ios::ate);
    
    if (file.is_open())
    {
        size = file.tellg();
        
        file.seekg(0, std::ios::beg);
 
        for (i=0, counter=0, j=0, pos=0; counter<3; i++)
        {
            file.read(&chrBuf, 1);
            pos++;

            if (terminalSequenceLength == 1 && 
                terminalSequence[0] != chrBuf &&
                (chrBuf == 10 || chrBuf == 13))
            {
                terminalSequence[1] = chrBuf;
                terminalSequenceLength = 2;
            }
            
            if (terminalSequenceLength == 0 && 
                (chrBuf == 10 || chrBuf == 13))
            {
                terminalSequence[0] = chrBuf;
                terminalSequenceLength = 1;
            }
            
            if (chrBuf == terminalSequence[0])
            {
                if (counter == 0 && !isComment)
                {
                    if (memblock[0] == 'Y' && memblock[1] == '7')
                        isGrayscale = false;
                    else if (memblock[0] == 'P' && memblock[1] == '5')
                        isGrayscale = true;
                    else
                    {
                        file.close();
                        printf("Not Y7 or P5 type\n");
                        return NULL;
                    }
                }
                if (counter == 1 && !isComment)
                {
                    for (k=0;
                         memblock[k] != ' ' && memblock[k] != '\0';
                         k++);
                    if (memblock[k] == ' ')
                    {
                        memblock[k] = '\0';
                        width = (int) atof(memblock);
                        height = (int) atof(&memblock[k+1]);
                    }
                }
                counter++;
                j = 0;
                isComment = false;
            }
            if (chrBuf == '#')
            {
                counter--;
                isComment = true;
            }
            if (chrBuf >= ' ')
            {
                memblock[j] = chrBuf;
                memblock[j+1] = '\0';
                if (++j >= 100)
                {
                    file.close();
                    printf("memblock buffer size (100 bytes) exceeded\n");
                    return NULL;
                }
            }
        }
        
        if (terminalSequenceLength == 2 && chrBuf == terminalSequence[0])
        {
            file.read(&chrBuf, 1);

            if (chrBuf == terminalSequence[1])
            {
                pos++;
            }
            else
            {
                printf("Unknown character in file\n");
                return NULL;
            }
        }
        
        if (width <= 0 || height <= 0)
        {
            file.close();
            printf("Width or height = 0\n");
            return NULL;
        }
        
        YUV = new char[width*height*(isGrayscale ? 1 : 2)];
        file.read(YUV, width*height*(isGrayscale ? 1 : 2));
        file.close();
    }
    
    if (isGrayscale)
        loadPGM = true;
    
    return YUV;
}

void show_uchar(int imnum, unsigned char * buf, int width, int height)
{
	unsigned char * rgb = new unsigned char[640*480*3];

	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++,l+=3)
		{
			rgb[l] = (unsigned char) (buf[k]);
			rgb[l+1] = (unsigned char) (buf[k]);
			rgb[l+2] = (unsigned char) (buf[k]);
		}
	}
  displayImage(imnum, rgb,height,width);
}


void show_uchar_maximized(int imnum, unsigned char * buf, int width, int height)
{
	unsigned char * rgb = new unsigned char[640*480*3];
	float min=1e10, max=-1e10;
	float num;
	unsigned char unum;

	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++)
		{
		  num = buf[k];
		  if (num<min)
		    min = num;
		  if (num>max)
		    max = num;
		}
	}
	
	if (max-min == 0)
	  max = min+1;
	
	printf("min=%.3f, max=%.3f\n", min, max);
	
	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++,l+=3)
		{
//		  num = (200.0 - 200.0*(((float)buf[k])-min)/(max-min));
      if (buf[k] == 0)
        unum = 0;
      else
        unum = 255;
			rgb[l] = (unsigned char) unum;
			rgb[l+1] = (unsigned char) unum;
			rgb[l+2] = (unsigned char) unum;
		}
	}
  displayImage(imnum, rgb,height,width);
}


void show_dispmap(int imnum, short * buf, int width, int height)
{
	unsigned char * rgb = new unsigned char[640*480*3];
	float scalefactor = 255.0/((float)(maxDisp-minDisp) * 100.0);
	float min=1e10, max=-1e10;
	float num;

	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++,l+=3)
		{
		  num = (unsigned char) ((buf[k]-minDisp*100)*scalefactor);
		  if (num<min)
		    min = num;
		  if (num>max)
		    max = num;
		  
			rgb[l] = (unsigned char) (buf[k]*scalefactor);
			rgb[l+1] = (unsigned char) (buf[k]*scalefactor);
			rgb[l+2] = (unsigned char) (buf[k]*scalefactor);
		}
	}
  displayImage(imnum, rgb,height,width);
//  printf("min=%.3f max=%.3f\n", min, max);
}


void show_float(int imnum, float * buf, int width, int height)
{
	unsigned char * rgb = new unsigned char[640*480*3];
	float min=1e10, max=-1e10;
	float num;

	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++)
		{
		  num = buf[k];
		  if (num<min)
		    min = num;
		  if (num>max)
		    max = num;
		}
	}
	
	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++,l+=3)
		{
			rgb[l] = (unsigned char) (255.0*(buf[k]-min)/(max-min));
			rgb[l+1] = (unsigned char) (255.0*(buf[k]-min)/(max-min));
			rgb[l+2] = (unsigned char) 0;
		}
	}
  displayImage(imnum, rgb,height,width);
}



int main(int argc,char **argv){
	int Nx,Ny;
	gimi::GIMI gim(1);
	TGIMI_PROTOCOL_GRID_HEADER head;
  TBitmap img0;
  TBitmap img1;
	int gridsize=100;
	
  float minX = -2.5;
  float maxX = 2.5;
  float minY = 0;
  float maxY = 5;
  float minZ = -0.5;
  float maxZ = 50;
  float unknownValue = -2.5;
  float cellsize;
  
  int handleLeft, handleRight, handleDisp;
  
	parse(argc,argv);
	/*
	int32_t size;    ///< The size of grid data in bytes
	int32_t Nx;      ///< The number of elements in x
	int32_t Ny;      ///< The number of elements in y
	float resolution;///< size of one cell in meters			
	float x;         ///< The global position of the grid origin 
	float y;         ///< The global position of the grid origin 
	float a;         ///< The global position of the grid origin 
	int32_t orig_x;  ///< The placement of the grid origin within the grid
	int32_t orig_y;  ///< The placement of the grid origin within the grid 
	unsigned char format;	///< The format of the grid data (GRID_FORMAT_TYPE_XXX)
	*/
	head.size = gridsize*gridsize*sizeof(grid_t);
	head.Nx = gridsize;
	head.Ny = gridsize;
	head.resolution = (maxX-minX)/gridsize;
	head.x = 50;
	head.y = 50;
	head.orig_x = gridsize/2;
  head.orig_y = gridsize/2;
	head.format = 1;	
	
  /// Initialize the debugging module
	debugInit();
	debugSetGlobalDebugLvl(paramDebugLevel);
	debugSetMode(DEBUG_MODE_TIMESTAMP_HH_MM_SS_MS);
  
	gim.connectToHub(paramHubAdress.c_str(), paramHubPort, paramOwnName.c_str());
	CGridGServerModule server(&gim);
	server.setHeader(head);

  CImageClient cli; ///< Image client interface
    
	if(!cli.initialize(&gim,paramImageServerName.c_str(),2,CIC_bitmap, 5000)){
			dPrint(1,"Image client initialization failed - aborting");
			exit(1);
	}

  if (showDisparity || showCameraImages)
  {
    int currentHandle = 0;
    if (showCameraImages)
    {
     	addImageWindow(640, 480);
    	addImageWindow(640, 480);
    	handleLeft = currentHandle++;
    	handleRight = currentHandle++;
    }
    if (showDisparity)
    {
    	addImageWindow(640, 480);
    	handleDisp = currentHandle++;
    }
  	glutImageGUI_Start();
  }

	float v;
	float phii=0;
	short * dispmap = new short[640*480];
	grid_t *data = new grid_t[gridsize*gridsize];

    char * leftOutput = new char[640*480];
    char * rightOutput = new char[640*480];
    char * leftInput, * rightInput;
    gim::StereoRoutines gsr;
    std::string dirname = "./";
    
    bool everythingOK;
    bool isCalculatedAlready = false;

    while(1)
    {
        everythingOK = true;

        if (loadFromFile)
        {
            char * leftImage = loadRawImage((dirname + "0_kuva" + imageNum + ".ppm").c_str());
            char * rightImage = loadRawImage((dirname + "1_kuva" + imageNum + ".ppm").c_str());
        
            if (leftImage != NULL && rightImage != NULL)
            {
                if (loadPGM)
                {
                    for (int i=0; i<640*480; i++)
                        leftOutput[i] = leftImage[i];
                    for (int i=0; i<640*480; i++)
                        rightOutput[i] = rightImage[i];
                }
                else
                {
                    gsr.fastRectifyImages(
                        leftImage, rightImage,
                        leftOutput, rightOutput,
                        640, 480,
                        rightOffset);
                }
                leftInput = leftOutput;
                rightInput = rightOutput;
                delete[] leftImage;
                delete[] rightImage;
            }
        }
        else
        {
         		if(!cli.getImageCopy(&img0,0))
 		        {
         				dPrint(1,"Failed to read stream 0");
         				everythingOK=false;
     		    }
        		if(!cli.getImageCopy(&img1,1))
        		{
        				dPrint(1,"Failed to read stream 1");
        				everythingOK=false;
    		    }
		    
<<<<<<< GridServer.cpp
		    leftInput = (char*)img0.data;
		    rightInput = (char*)img1.data;

    		for (int i=0; i<2; i++)
    		{
    		    char buf[100];
        		switch(gim.unsubscribeService(paramImageServerName, CIC_bitmap, i, 15000))
=======
    		    leftInput = (char*)img0.data;
    		    rightInput = (char*)img1.data;
/*
        		for (int i=0; i<2; i++)
>>>>>>> 1.11
        		{
        		    char buf[100];
            		switch(gim.unsubscribeService(paramImageServerName, CIC_bitmap, i, 5000))
            		{
            		    case GIMI_OK: sprintf(buf, "Unsubscribe %d: GIMI_OK", i); break;
            		    case GIMI_INVALIDCLIENT: sprintf(buf, "Unsubscribe %d: GIMI_INVALIDCLIENT", i); break;
            		    case GIMI_INVALIDTYPE: sprintf(buf, "Unsubscribe %d: GIMI_INVALIDTYPE", i); break;
            		    case GIMI_TIMEOUT: sprintf(buf, "Unsubscribe %d: GIMI_TIMEOUT", i); break;
            		    case GIMI_NOCONNECTION: sprintf(buf, "Unsubscribe %d: GIMI_NOCONNECTION", i); break;
            		    case GIMI_ERROR: sprintf(buf, "Unsubscribe %d: GIMI_ERROR", i); break;
            		}
            		dPrint(1,buf);
            }
*/
        }
        
        for (int j=0, k=0; j<480; j++, k+=rightOffset)
          for (int i=0; i<640-rightOffset; i++, k++)
          {
              rightInput[k] = rightInput[k+rightOffset];
          }
        
        if (showCameraImages)
        {
            show_uchar(handleLeft, (unsigned char *) leftInput, 640, 480);
            show_uchar(handleRight, (unsigned char *) rightInput, 640, 480);
        }

#ifdef SHOW_WAVES
// Simulated elevation map:
        for(int j=0,k=0;j<gridsize;j++)
        {
            v = 0.5*sin(phii+(float)j/10.0);
            for(int i=0;i<gridsize;i++,k++)
            {
                data[k] = v;
            }
        }

        usleep(3000*1000);
        phii+=0.2;
#else
// Real elevation map:
    		if (everythingOK && !isCalculatedAlready)
            gim::calculateHeightMap(
                leftInput, rightInput,
                640, 480,
                data,
                minX, maxX, gridsize,
                minY, maxY, gridsize,
                minZ, maxZ, unknownValue,
                cellsize, 50,
                minNumBuf,
                minStDev,
                stDevWinMin,
                stDevWinMax,
                valueIndex,
                minDisp,
                maxDisp,
                dispmap);    		
#endif

        if (showDisparity)
        {
            show_dispmap(handleDisp, dispmap, 640, 480);
        }
	      if (everythingOK)
	      {
            if(server.sendGrid((unsigned char *) data))
            {
                fprintf(stderr,"Succesfully sent data!\n");
            }
        }
        
        if (loadFromFile)
        {
            isCalculatedAlready = true;
        }
/*
            while (1)
            {
                if (showCameraImages)
                {
                    show_uchar(handleLeft, (unsigned char *) leftInput, 640, 480);
                    show_uchar(handleRight, (unsigned char *) rightInput, 640, 480);
                }
                if (showDisparity)
                {
                    show_dispmap(handleDisp, dispmap, 640, 480);
                }
                usleep(500*1000);
            }
        }
	      else
*/
    	      usleep(300*1000);
/*	      
    		for (int i=0; i<2; i++)
    		{
    		    char buf[100];
        		switch(gim.subscribeToService(paramImageServerName, CIC_bitmap, i, 15000))
        		{
        		    case GIMI_OK: sprintf(buf, "Subscribe %d: GIMI_OK", i); break;
        		    case GIMI_INVALIDCLIENT: sprintf(buf, "Subscribe %d: GIMI_INVALIDCLIENT", i); break;
        		    case GIMI_INVALIDTYPE: sprintf(buf, "Subscribe %d: GIMI_INVALIDTYPE", i); break;
        		    case GIMI_TIMEOUT: sprintf(buf, "Subscribe %d: GIMI_TIMEOUT", i); break;
        		    case GIMI_NOCONNECTION: sprintf(buf, "Subscribe %d: GIMI_NOCONNECTION", i); break;
        		    case GIMI_ERROR: sprintf(buf, "Subscribe %d: GIMI_ERROR", i); break;
        		}
        		dPrint(1,buf);
        }
*/
    }

  	delete[] data;
  	delete[] dispmap;
  	delete[] leftOutput;
  	delete[] rightOutput;
  	
    return 0;
}
